/*
  Author  : Jihoon Lee
  Date      : Feb. 2012

  ObjectManager.cpp
 */

#include <object_manager/ObjectManager.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Bool.h>

#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <assert.h>

double X_CALIBRATION_HACK = -0.01;
double Y_CALIBRATION_HACK = -0.02;

double TOOLBOX_Y_THRESH = -0.35;  // define toolbox area and active work area (toolbox area is defined as y < TOOLBOX_Y_THRESH in /base_link)

bool DO_TABLE_INTERPOLATION = true;

void interpolate_table_coords(double x, double y, double &x_out, double &y_out) {
  // --- setup grid ---
  double bi_grid[6][3];
  // 1    3    5             ^ x
  //                         |
  // 0    2    4    <-----y  |
  // 0: lower left (x=0.3, y=0.5)
  bi_grid[0][1] = 0.3;  //.32; // x1
  bi_grid[0][0] = 0.5;  //.36; // x2
  bi_grid[0][2] = 0.01;  //-.01; //-.02;  // x shift
  // 1: upper left (x=0.8, y=0.5)
  bi_grid[1][1] = 0.8;  //.55; // x1
  bi_grid[1][0] = 0.5;  //0.26; // x2
  bi_grid[1][2] = 0.005; //-.01;  // x shift
  // 2: lower right (x=0.3, y=-0.6)
  bi_grid[2][1] = 0.3;  //.36; // x1
  bi_grid[2][0] = -0.6; //-.32; // x2
  bi_grid[2][2] = -0.04;  // x shift
  // 3: upper right (x=0.8, y=-0.6)
  bi_grid[3][1] = 0.8;  //.67; // x1
  bi_grid[3][0] = -0.6; //-.30; // x2
  bi_grid[3][2] = -0.02;  //0.01; //0.03;  // x shift

  // --- interpolate in y ---
  // bottom y
  double a1 = (bi_grid[2][0]-y)/(bi_grid[2][0]-bi_grid[0][0])*bi_grid[0][2] +
    (y-bi_grid[0][0])/(bi_grid[2][0]-bi_grid[0][0])*bi_grid[2][2];
  // top y
  double a2 = (bi_grid[3][0]-y)/(bi_grid[3][0]-bi_grid[1][0])*bi_grid[1][2] +
    (y-bi_grid[1][0])/(bi_grid[3][0]-bi_grid[1][0])*bi_grid[3][2];

  // figure out x,y for r1, r2
  double x1 = 0.3;
  double y1 = y;
  double x2 = 0.8;
  double y2 = y;

  // --- interpolate in x ---
  double b = (x2-x)/(x2-x1)*a1 + (x-x1)/(x2-x1)*a2;

  //fprintf(stderr, "a1: %f\t a2: %f\t b: %f\n", a1, a2, b);
  //fprintf(stderr, "y1: %f\t y2: %f\n", y1, y2);
  x_out = x + b;
  y_out = y; /// + b;
  //fprintf(stderr, "x: %f y: %f\n", x, y);
  //fprintf(stderr, "x_out: %f y_out: %f\n", x_out, y_out);
}

ObjectManager::ObjectManager(int argc,char** argv)
{
  idCount = 0;
  update_status = true;

  std::string service_name;

  // to call tabletop segmentation service
  nh.param<std::string>("segmentation_srv",service_name, segment_srv_name);
  while (!ros::service::waitForService(service_name, ros::Duration(2.0)) && nh.ok())
  {
    ROS_INFO("Waiting for %s service to come up",service_name.c_str());
  }
  if(!nh.ok()) exit(0);
  segmentation_srv =nh.serviceClient<tabletop_segmentor::TabletopSegmentation>(service_name,true);

  // to call find_cluster bounding box service
  nh.param<std::string>("find_bounding_srv",service_name, find_bounding_box_name);
  while (!ros::service::waitForService(service_name, ros::Duration(2.0)) && nh.ok())
  {
    ROS_INFO("Waiting for %s service to come up",service_name.c_str());
  }
  if(!nh.ok()) exit(0);
  find_bounding_box_srv = nh.serviceClient<object_manipulation_msgs::FindClusterBoundingBox2>(service_name,true);


  // To process a request from block_build_manager
  nh.param<std::string>("update_table_objects_topic",service_name, "/blocknlp/update_table");
  update_table_sub = nh.subscribe(service_name,5,&ObjectManager::objectServerCallback,this);

  // To process a table request.
  nh.param<std::string>("request_table_srv",service_name, "/blocknlp/request_table");
  table_request_srv_server = nh.advertiseService<object_manager::RequestTable::Request,object_manager::RequestTable::Response>(service_name, boost::bind(&ObjectManager::tableRequestCallback,this,_1,_2));

  // To update an object's status as being in or out of the toolbox and whether or not it is in hand
  nh.param<std::string>("update_object_topic",service_name, "/blocknlp/update_object");
  update_object_sub = nh.subscribe(service_name,5,&ObjectManager::updateObjectCallback,this);

  // To pause and unpause updating the object manager based on a command from the head manager
  nh.param<std::string>("object_manager_switch_topic",service_name, "/mit/object_manager_switch");
  object_manager_switch_sub = nh.subscribe(service_name,5,&ObjectManager::objectManagerSwitchCallback,this);

  // publish marker information
  marker_publisher = nh.advertise<visualization_msgs::MarkerArray>("/mit/object_marker_array", 100); //boundingmarkers",100);

  // publish object information
  //object_publisher = nh.advertise<object_manager::ObjectList>("/blocknlp/object_list",100);
  object_publisher = nh.advertise<object_manager::ObjectList>("/mit/object_list", 100);                  // publishes only the blocks in the active work area
  object_global_publisher = nh.advertise<object_manager::ObjectList>("/mit/object_list_global", 100);    // publishes all blocks in the global frame
  object_all_publisher = nh.advertise<object_manager::ObjectList>("/mit/object_list_all", 100);          // publishes all blocks (toolbox and active) in the base_link frame

  // debugging purpose
  point_publisher = nh.advertise<PointCloud>("segmented_points",100);
}

void ObjectManager::callTabletopSegmentation(tabletop_segmentor::TabletopSegmentation& segmentation)
{
  //ROS_INFO("Calling Tabletop segmentation service");
  if(!segmentation_srv.call(segmentation))
  {
    ROS_ERROR("Call to segmentation service failed");
  }
  if(segmentation.response.result != segmentation.response.SUCCESS)
  {
    ROS_ERROR("Segmentation failed");
    return;
  }
  //ROS_INFO("Segmentation service succeeded. Detected %d clsusters",(int)segmentation.response.clusters.size());
  return;
}

double
rgb2h(double nR, double nG, double nB) {
  //fprintf(stderr, "r: %f g: %f b: %f\n", nR, nG, nB);
  double m = std::min(nR,nG);
  m = std::min(m,nB);
  double M = std::max(nR,nG);
  M = std::max(M,nB);
  double H = 0;
  double S = 0;
  //fprintf(stderr, "M: %f m: %f\n", M,m);
  if (M!=m) {
    double f = 0;
    if (nR==m) {
      f = (nG-nB);
    }
    else {
      if (nG==m)
	f = (nB-nR);
      else
	f = (nR-nG);
    }
    double i = 0;
    if (nR==m)
      i = 3;
    else {
      if (nG==m)
	i = 5;
      else
	i = 1;
    }
    H = (i-f/(M-m));
    if (H>=6)
      H = H - 6;
    H *= 60;
    S = (M-m)/M;
  }
  //fprintf(stderr, "H: %f\n", H);
  return H;
}

// data taken on 4th floor
// orange: 12-24
// green: 120-210
// blue: 227-233
// purple: 250-275
// red: 338-356


std::string
h2str(double h) {
  std::string s;
  if (h < 7)
    s = "red";
  else if (h < 38)
    s = "orange";
  else if (h < 76)
    s = "yellow";
  else if (h < 215)
    s = "green";
  else if (h < 240)
    s = "blue";
  else if (h < 300)
    s = "purple";
  else if (h < 360)
    s = "red";
  else
    s = "mystery";
  return s;
}


void ObjectManager::extractObject(std::vector<sensor_msgs::PointCloud2> clusters)
{
  static ros::Time last_update = ros::Time::now();
  ros::Duration delta = ros::Time::now() - last_update;
  //std::cout << "extractObject delta_t: " << delta.toSec() << "\n";
  last_update = ros::Time::now();


  object_manipulation_msgs::FindClusterBoundingBox2 fbb;
  object_manager::Object obj;
  std::vector<object_manager::Object> new_obj_list;
  std_msgs::ColorRGBA color;

  for(unsigned int i =0; i < clusters.size(); i++)
  {
    fbb.request.cluster = clusters[i];

    if(!find_bounding_box_srv.call(fbb))
    {
      ROS_ERROR("Call to find_bounding_box service failed");
    }

    if(fbb.response.error_code != fbb.response.SUCCESS)
    {
      ROS_ERROR("Find Bounding Box failed");
      return;
    }
    //    ROS_INFO("Find Bounding Box succeeded.");

    extractColor(color,clusters[i]);
    makeObject(obj,fbb.response,color,idCount++);
    // only update the object list if the object is within the active work area
    if (obj.pose.position.y > TOOLBOX_Y_THRESH)
      new_obj_list.push_back(obj);

    // add into object list
  }

  // tbd - prentice
  //removeOldMarkers();

  updateObjectList(new_obj_list);
}

void ObjectManager::extractColor(std_msgs::ColorRGBA& color,sensor_msgs::PointCloud2& p_cloud)
{
  unsigned int i;
  unsigned int rgb = 0;
//  unsigned int max_val;
//  unsigned int c;
  unsigned int r,g,b,size;

  r=g=b= 0;

  std::map<unsigned int,unsigned int> histogram;
  std::map<unsigned int,unsigned int>::iterator it;
  pcl::PointCloud<pcl::PointXYZRGB> cloud;
  pcl::fromROSMsg(p_cloud,cloud);

  size = cloud.size() * 255;

  for(i =0; i < cloud.points.size(); i++)
  {
    rgb = *reinterpret_cast<int*>(&(cloud.points[i].rgb));

    r += ((rgb >> 16) & 0xff);
    g += ((rgb >> 8) & 0xff);
    b += ((rgb) & 0xff);
  }

  color.r = (float) r/ (float)size;
  color.g = (float) g/ (float)size;
  color.b = (float) b/ (float)size;

  /*
  color.g = 0.0;
  if(r > b)
  {
    color.r = 1.0;
    color.b = 0.0;
  }
  else {
    color.b = 1.0;
    color.r = 0.0;
  }*/

  /*
  // extract rgba from 32 bit
  // red
  c = ((rgb >> 16) & 0xff);
  color.r = (float)((float)c / 255);

  // green
  c = ((rgb >> 8) & 0xff);
  color.g = (float)((float)c / 255);

   // blue
   c = ((rgb) & 0xff);
   color.b = (float)((float)c / 255);

   color.g = 0;
   if(color.r > color.b)
   {
     color.r = 1.0f;
     color.b = 0.0f;
   }
   else {
     color.r = 0.0f;
     color.b = 1.0f;
   }
*/

   // alpha
   color.a = 1.0f;
}



// TBD - make updateMarkers function that doesnt delete/add, but updates instead


void ObjectManager::updateObjectList(std::vector<object_manager::Object> new_list)
{
  unsigned int i,j,a;
  std_msgs::ColorRGBA color;
  bool associated[object_list.size()];
  for (i=0; i < object_list.size(); i++) {
    associated[i] = false;
    // also clear out visibility
    object_visibility_mask[i].visible = false;
  }
  float dist;
  float hue_dist, hue_i, hue_j;
  double THRESH = 0.07; //0.02;
  double HUE_THRESH = 12;

  // initialization - capture N frames and then use these templates as the only objects
  int NUM_INIT_UPDATES = 10;
  static int initialize_count = 0;
  initialize_count++;
  //printf("\n-----\nUPDATE OBJECT LIST (%d): num objects %d, new objects %d\n", initialize_count, object_list.size(), new_list.size());


  ///////
  // for first N frames, build the object list
  //   - handles a few faulty initial frames to build core set of objects
  if (initialize_count < NUM_INIT_UPDATES) {
    printf("INITIALIZING\n");

    // if first object list, just copy and done
    if(object_list.size() == 0) {
      object_list.insert(object_list.begin(),new_list.begin(),new_list.end());
      for (i=0; i<object_list.size(); i++) {
	// handle draw type
	object_draw_type.push_back(visualization_msgs::Marker::ADD);
	// handle viz_list
	visibility_mask viz;
	viz.visible = true;
	viz.last_time_seen = ros::Time::now();
	object_visibility_mask.push_back(viz);
      }
      return;
    }

    for(i=0; i<object_list.size(); i++) {
      associated[i] = false;
      hue_i = rgb2h(object_list[i].color.r,
		    object_list[i].color.g,
		    object_list[i].color.b);

      for(j=0; j<new_list.size(); j++) {
	// compute distance between object i and new_object j
	dist = sqrt( pow(new_list[j].pose.position.x-object_list[i].pose.position.x, 2) +
		     pow(new_list[j].pose.position.y-object_list[i].pose.position.y, 2) +
		     pow(new_list[j].pose.position.z-object_list[i].pose.position.z, 2) );
	hue_j = rgb2h(new_list[j].color.r,
		      new_list[j].color.g,
		      new_list[j].color.b);
	hue_dist = fabs(hue_i - hue_j);

	std::cout << "dist " << i << "_" << j << " = " << dist << "\n";
	std::cout << "HUE i: " << hue_i << " j: " << hue_j << " hue_dist " << hue_dist << "\n";

	// associate objects if sufficiently close
	if(dist <= THRESH && hue_dist <= HUE_THRESH) {
	  std::cout << "Associated object " << i << " with new_object " << j << "\n";

	  // update object with new_object's contents
	  object_list[i].pose = new_list[j].pose;
	  object_list[i].color = new_list[j].color;
	  // mark as associated
	  associated[i] = true;
	  // remove object from new_list
	  new_list.erase(new_list.begin() + j);

	  // note: we break so don't need to update loop index
	  // j--;

	  // mark in viz_list
	  object_visibility_mask[i].visible = true;
	  object_visibility_mask[i].last_time_seen = ros::Time::now();
	  // mark draw_type
	  object_draw_type[i] = visualization_msgs::Marker::MODIFY;
	  break;
	}
      }
      // if old object was not associated with new object, update it's visibility to false
      if ( !associated[i] ) {
	object_visibility_mask[i].visible = false;
	object_draw_type[i] = visualization_msgs::Marker::MODIFY;
      }

      // if obj is not inhand and not updated. remove from object list
      //if(updated == false && object_list[i].inhand == false)
      // object_list.erase(object_list.begin() + i);
      //i--;
    }

    // add new objects (ones that weren't associated)
    for(j=0; j<new_list.size(); j++) {
	object_list.push_back(new_list[j]);
	object_draw_type.push_back(visualization_msgs::Marker::ADD);
	visibility_mask viz;
	viz.visible = true;
	viz.last_time_seen = ros::Time::now();
	object_visibility_mask.push_back(viz);
    }

    return;
  }
  ///

  // at 10th iteration, clean up any noise (i.e. remove redundant objects and lock in)
  if (initialize_count == NUM_INIT_UPDATES) {
    std::cout << "Cleaning up object initialization\n";
    for (i=0; i < object_list.size(); i++) {
      for (j=i+1; j < object_list.size(); j++) {
	dist = sqrt( pow(object_list[j].pose.position.x-object_list[i].pose.position.x, 2) +
		     pow(object_list[j].pose.position.y-object_list[i].pose.position.y, 2) +
		     pow(object_list[j].pose.position.z-object_list[i].pose.position.z, 2) );
	if (dist < 0.1) {
	  std::cout << " - object " << i << " and " << j << " are too close (" << dist << ") - REMOVING j\n";
	  object_list.erase(object_list.begin() + j);
	  j--;
	}
      }
    }
    // finalize labels
    for (i=0; i < object_list.size(); i++) {
      if(object_list[i].label.empty()){
	object_list[i].label = h2str( rgb2h(object_list[i].color.r,
					    object_list[i].color.g,
					    object_list[i].color.b) );
      }
    }

    std::cout << "DONE with initialization.\n\n\n";
  }


  // hack - if an object is in hand, don't update list
  for (i=0; i < object_list.size(); i++) {
    if (object_list[i].inhand) {
      std::cout << "*********************************************" << "\n";
      std::cout << "Object " << i << " is INHAND, skipping update!" << "\n";
      std::cout << "*********************************************" << "\n";
      return;
    }
  }

  // compute pairwise xyz and hue distances
  double **dist_ij = new double*[object_list.size()];
  for(i=0; i < object_list.size(); ++i)
    dist_ij[i] = new double[new_list.size()];
  double xyz_dist;
  for(i=0; i < object_list.size(); i++) {
    hue_i = rgb2h(object_list[i].color.r, object_list[i].color.g, object_list[i].color.b);
    for(j=0; j < new_list.size(); j++) {
      xyz_dist = sqrt( pow(new_list[j].pose.position.x-object_list[i].pose.position.x, 2)
		       + pow(new_list[j].pose.position.y-object_list[i].pose.position.y, 2)
		       );//+ pow(new_list[j].pose.position.z-object_list[i].pose.position.z, 2) );
      hue_j = rgb2h(new_list[j].color.r, new_list[j].color.g, new_list[j].color.b);

      // set hue distance to 0 if block has just been transfered to the active work area and the color labels match
      if (object_list[i].color.r == -1 && object_list[i].label == h2str(hue_j)) hue_dist = 0.0f;
      else hue_dist = fabs(hue_i - hue_j);

      dist = xyz_dist + hue_dist/100.0;      // we'll treat hue as centimeters
      dist_ij[i][j] = dist;
      //printf("dist_ij: i=%d j=%d d=%f (%f)\n", i,j,dist,dist_ij[i][j]);
    }
  }

  /*printf("DIST MATRIX\n");
  for(i=0; i < object_list.size(); i++) {
    printf("i=%d\t", i);
    for(j=0; j < new_list.size(); j++) {
      printf("j=%d:%f\t", j, dist_ij[i][j]);
    }
    printf("\n");
  }
  printf("\n");*/

  int *association = new int[new_list.size()];
  std::vector<int> conflict;
  double ASSOC_THRESH = 0.2;
  for(j=0; j<new_list.size(); j++) {
    // find min(i)_j
    double min_val = ASSOC_THRESH; //10000;
    int min_i = -1;
    for(i=0; i < object_list.size(); i++) {
 // only associate the new object if its height is smaller than a certain value to prevent reassociation of stacked objects
      if (dist_ij[i][j] < min_val && new_list[j].dim.z < 0.05) {
	min_val = dist_ij[i][j];
	min_i = i;
      }
    }
    association[j] = min_i;
    if (min_i >= 0) {
      if (associated[min_i] == true) {
	// already associated, so we have a conflict
	printf("CONFLICT: multiple new pointclouds are associating with same object %d\n", min_i);
	conflict.push_back(min_i);
      }
      associated[min_i] = true;
    }
  }

  // conflict resolution
  for(a=0; a < conflict.size(); a++) {
    i = conflict[a];
    // find min of conflicting nodes
    double min_val = 10000;
    int min_j = -1;
    for(j=0; j < new_list.size(); j++) {
      if (association[j] == i) {
	if (dist_ij[i][j] < min_val) {
	  min_val = dist_ij[i][j];
	  min_j = j;
	}
      }
    }
    for(j=0; j < new_list.size(); j++) {
      if (association[j] == i) {
	if (j != min_j) {
	  // this is a loser, set to -1
	  association[j] = -1;
	}
      }
    }
  }

  // update object attributes
  for(j=0; j < new_list.size(); j++) {
    if (association[j] >= 0) {
      // copy new object attributes over to corresponding object
      i = association[j];
      object_list[i].pose = new_list[j].pose;
      object_list[i].color = new_list[j].color;
      object_visibility_mask[i].visible = true;
      object_visibility_mask[i].last_time_seen = ros::Time::now();
      object_draw_type[i] = visualization_msgs::Marker::MODIFY;
    } else {
      // ignore new object, lost a conflict resolve
    }
  }


}


/* original
void ObjectManager::updateObjectList(std::vector<object_manager::Object> new_list)
{
  unsigned int i,j;
  //unsigned int id;
  std_msgs::ColorRGBA color;
  bool updated;
  float dist;visualization_msgs::Marker::ADD;

  if(object_list.size() == 0) {
    object_list.insert(object_list.begin(),new_list.begin(),new_list.end());
    return;
  }

  for(i = 0;i <object_list.size(); i++)
  {
    updated = false;
//    printf("Id = %d\n",object_list[i].id);
    for(j =0; j < new_list.size(); j++) 
    {
      dist =0;
      dist += fabs(new_list[j].pose.position.x - object_list[i].pose.position.x);
      dist += fabs(new_list[j].pose.position.y - object_list[i].pose.position.y);
//      dist += fabs(new_list[j].pose.position.z - object_list[i].pose.position.z);

      if(dist < 0.1) {
//        id = object_list[i].id;
//        color = object_list[i].color;
//        object_list[i] = new_list[j];
//        object_list[i].id = id;
//        object_list[i].color;
        updated = true;
        new_list.erase(new_list.begin() + j);

//        printf("Old ID = %d New ID = %d  DIST = %f\n",object_list[i].id,new_list[j].id,dist);
        break;
      }
    }

    // if obj is not inhand and not updated. remove from object list
    if(updated == false && object_list[i].inhand == false)
    {
//      printf("ID = %d is erased\n",object_list[i].id);
      object_list.erase(object_list.begin() + i);
      i--;
    }
  }

  // add new objects
  for(i =0; i < new_list.size();i ++)
  {
//    printf("ID = %d is added\n",new_list[i].id);
    object_list.push_back(new_list[i]);
  }

//  std::cout << "=======================" << std::endl;
}
*/



void ObjectManager::makeObject(object_manager::Object& obj,object_manipulation_msgs::FindClusterBoundingBox2::Response& response,std_msgs::ColorRGBA color,unsigned int i)
{
  obj.header = response.pose.header;
  obj.header.stamp = ros::Time::now();

  obj.id     = i; 
  obj.pose   = response.pose.pose;

  // TBD - Calibration HACK
  obj.pose.position.x += X_CALIBRATION_HACK;
  obj.pose.position.y += Y_CALIBRATION_HACK;

  // TBD2
  if (DO_TABLE_INTERPOLATION) {
    //fprintf(stderr, "BEFORE: %f %f\n", obj.pose.position.x, obj.pose.position.y);
    interpolate_table_coords(obj.pose.position.x, obj.pose.position.y,
			     obj.pose.position.x, obj.pose.position.y);
    //fprintf(stderr, "AFTER: %f %f\n", obj.pose.position.x, obj.pose.position.y);
  }

  obj.dim    = response.box_dims;
  obj.color = color;

  //obj.color.r = obj.dim.x * 10;
  //obj.color.g = obj.dim.y * 10;
  //obj.color.b = obj.dim.z * 10;
  //obj.color.a = 1.0f;

  obj.inhand = false;
  obj.atHome = false;
  obj.homePosition.position.x = obj.pose.position.x;
  obj.homePosition.position.y = obj.pose.position.y;
  obj.homePosition.position.z = obj.pose.position.z;
  obj.homePosition.orientation.x = 0;
  obj.homePosition.orientation.y = 0;
  obj.homePosition.orientation.z = 0;
  obj.homePosition.orientation.w = 1;
}

// Define a hard-coded toolbox object and adds it to the list of objects in the toolbox
void ObjectManager::makeToolBoxObject(float x, float y, float z, std::string color_str)
{
  assert(y <= TOOLBOX_Y_THRESH);  // check to see if (x,y,z) position resides in the toolbox area
  object_manager::Object obj;
  std_msgs::ColorRGBA color;

  obj.header.seq = 0;
  obj.header.stamp = ros::Time::now();
  obj.header.frame_id = "/base_link";

  obj.id = idCount++; 

  geometry_msgs::Pose initial_pose;
  initial_pose.position.x = x;
  initial_pose.position.y = y;
  initial_pose.position.z = z;
  initial_pose.orientation.x = 0;
  initial_pose.orientation.y = 0;
  initial_pose.orientation.z = 0;
  initial_pose.orientation.w = 1;

  obj.pose   = initial_pose;
  obj.homePosition = initial_pose;

  obj.dim.x = 0.044;
  obj.dim.y = 0.044;
  obj.dim.z = 0.044;

  extractColorFromName(color,color_str);
  obj.color = color;
  obj.label = color_str;

  obj.inhand = false;
  obj.atHome = true;

  object_list_toolbox.push_back(obj);
}

// Define the initial configuration of the toolbox. The y coordinates of the blocks in the toolbox
// must be smaller than TOOLBOX_Y_THRESH
void ObjectManager::initializeToolbox()
{
  const float Z_POS = 0.660;  //define the z coordinate of the toolbox blocks relative to /base_link
  //makeToolBoxObject(0.6, -0.45, Z_POS, "blue");
  //makeToolBoxObject(0.5, -0.45, Z_POS, "red");
  //makeToolBoxObject(0.4, -0.45, Z_POS, "green");
  //makeToolBoxObject(0.3, -0.45, Z_POS, "orange");
  makeToolBoxObject(0.63, -0.55, Z_POS, "blue");
  makeToolBoxObject(0.545, -0.55, Z_POS, "red");
  makeToolBoxObject(0.46, -0.55, Z_POS, "green");
  makeToolBoxObject(0.375, -0.55, Z_POS, "orange");
  makeToolBoxObject(0.29, -0.55, Z_POS, "purple");
}
  

// Convert a string to a std_msgs::ColorRGBA object (Used by makeToolBoxObject)
void ObjectManager::extractColorFromName(std_msgs::ColorRGBA& color, std::string str)
{
  unsigned int r = 0, g = 0, b = 0;
  if (str == "red") {r = 255;}
  else if (str == "orange") {r = 255; g = 165;}
  else if (str == "yellow") {r = 255; g = 255;}
  else if (str == "green") {g = 128;}
  else if (str == "blue") {b = 255;}
  else if (str == "purple") {r = 128; b = 128;}
  else {r = 128; g = 128; b = 128;}  // gray

  color.r = (float) r;
  color.g = (float) g;
  color.b = (float) b;
  color.a = 1.0f;
}


void ObjectManager::processScene()
{
  // get point cloud cluster
  tabletop_segmentor::TabletopSegmentation segmentation;
  // call tabletop segmentation service
  callTabletopSegmentation(segmentation);
  // setting table
  table = segmentation.response.table;
  //publishSegmentedPoints(segmentation.response.clusters);
  extractObject(segmentation.response.clusters);
}


void ObjectManager::objectServerCallback(const std_msgs::Empty::ConstPtr& msg)
{
  if(update_status){
    //ROS_INFO("in objectserver callback");

    static ros::Time last_update = ros::Time::now();
    ros::Duration delta = ros::Time::now() - last_update;
    std::cout << "objectServerCallback delta_t: " << delta.toSec() << "\n";
    last_update = ros::Time::now();

    processScene();
  }
}

void ObjectManager::objectManagerSwitchCallback(const std_msgs::Bool::ConstPtr& msg)
{
  ROS_INFO("in objectManagerSwitch callback");
  update_status = msg->data;
}


void ObjectManager::updateObjectCallback(const object_manager::Object::ConstPtr& msg)
{
//  ROS_INFO("in update object callback");
  object_manager::Object new_obj = *msg;
//  object_list.clear();

  // move object into the active work area if it is currently in the toolbox
  if(new_obj.atHome == false){
    for(unsigned int i = 0; i < object_list_toolbox.size(); i++) {
      if(object_list_toolbox[i].id == new_obj.id) {
	object_list_toolbox[i].header.stamp = ros::Time::now();
	object_list_toolbox[i].atHome = false;
        object_list_toolbox[i].color.r = -1;    // flag to associate block by label instead of hue distance in the active area
	object_list.push_back(object_list_toolbox[i]);
	object_list_toolbox.erase(object_list_toolbox.begin()+i);

        // handle draw type
        object_draw_type.push_back(visualization_msgs::Marker::ADD);
        // handle viz_list
        visibility_mask viz;
        viz.visible = true;
        viz.last_time_seen = ros::Time::now();
        object_visibility_mask.push_back(viz);
      }
    }
  }

  // Update the inhand field of the object
  if(new_obj.inhand == false) {
    //
    // === just put down an object ===
    //
    bool flag = true;

    for(unsigned int i = 0; i < object_list.size(); i++)
    {
      if(object_list[i].inhand == true)
      {
	std::cout << "*********************************************" << "\n";
	std::cout << "PUT DOWN object " << i << "\n";
	std::cout << "*********************************************" << "\n";
        //object_list.erase(object_list.begin() + i);

	// the new_obj is in /r_gripper_tool frame
        //object_list[i].header.frame_id = new_obj.header.frame_id;
        //object_list[i].pose = new_obj.pose;
	// we want /base_link frame, so use the most current tf
        object_list[i].header.stamp = ros::Time::now();

	geometry_msgs::PoseStamped ps_in, ps_out;
	ps_in.header = object_list[i].header;
	ps_in.pose = object_list[i].pose;
	// malformed quat error --- try resetting
	ps_in.pose.orientation.x = 0;
	ps_in.pose.orientation.y = 0;
	ps_in.pose.orientation.z = 0;
	ps_in.pose.orientation.w = 1.0;
	//tf::TransformListener::transformPose("/base_link", ps_in, ps_out);
	listener.waitForTransform("/base_link", object_list[i].header.frame_id, ros::Time::now(), ros::Duration(3.0));
	listener.transformPose("/base_link", ps_in, ps_out);
	printf("POSE OUT: x: %f y: %f z: %f\n", ps_out.pose.position.x, ps_out.pose.position.y, ps_out.pose.position.z);
	object_list[i].header.frame_id = "/base_link";
	object_list[i].pose = ps_out.pose;

	// object_list[i].header.frame_id = "/base_link";
	// object_list[i].pose.position.x = gripper2base_transform.getOrigin().x();
	// object_list[i].pose.position.y = gripper2base_transform.getOrigin().y();
	// object_list[i].pose.position.z = gripper2base_transform.getOrigin().z();
	// printf("POSE OUT 2: x: %f y: %f z: %f\n", object_list[i].pose.position.x, object_list[i].pose.position.y, object_list[i].pose.position.z);

        object_list[i].inhand = new_obj.inhand; // i.e. false
        break;
      }
    }

    //    processScene();
    /*
    while(flag && ros::ok()) {
      for(unsigned int i = 0; i < object_list.size(); i++)
	{
	  ROS_INFO("i = %d",object_list[i].id);
	  if(new_obj.color.r == object_list[i].color.r && new_obj.color.b == object_list[i].color.b)
	    {
	      flag = false;
	      object_list[i].header.stamp = ros::Time::now();
	      object_list[i].id = new_obj.id;
	      break;
	    }
	}
      ros::spinOnce();
      ros::Duration(0.05f).sleep();
      ROS_INFO("Here");
      }*/

  }
  else {
    //
    // === just PICKED UP an object ===
    //    processScene();
    std::cout << "*********************************************" << "\n";
    std::cout << "Picked Up object " << new_obj.id << "\n";
    std::cout << "  - position: x:" << new_obj.pose.position.x << " y:" << new_obj.pose.position.y << " z:" << new_obj.pose.position.z << "\n";
    std::cout << "*********************************************" << "\n";
    for(unsigned int i = 0; i < object_list.size(); i++) {
      if(object_list[i].id == new_obj.id) {
        object_list[i].header.frame_id = new_obj.header.frame_id;
        object_list[i].header.stamp = ros::Time::now();
        object_list[i].pose = new_obj.pose;
        object_list[i].inhand = new_obj.inhand;

	// tbd - update object_visibility, object_draw
      }
    }
  }

  // move object back into the toolbox area if it is currently in the active area
  if(new_obj.atHome == true) {
    for(unsigned int i = 0; i < object_list.size(); i++) {
      if(object_list[i].id == new_obj.id){
	object_list[i].header.stamp = ros::Time::now();
	object_list[i].atHome = true;

	// reset color to hard-coded rgb values
  	std_msgs::ColorRGBA color;
  	extractColorFromName(color,object_list[i].label);	
        object_list[i].color = color;
        
        object_list[i].pose = object_list[i].homePosition;
        object_list[i].dim.x = 0.044;
        object_list[i].dim.y = 0.044;
        object_list[i].dim.z = 0.044;
        object_list[i].header.stamp = ros::Time::now();

	object_list_toolbox.push_back(object_list[i]);
	object_list.erase(object_list.begin()+i);
        object_draw_type.erase(object_draw_type.begin()+i);
        object_visibility_mask.erase(object_visibility_mask.begin()+i);
      }
    }
  }
}

/* original
void ObjectManager::updateObjectCallback(const object_manager::Object::ConstPtr& msg)
{
//  ROS_INFO("in update object callback");
  object_manager::Object new_obj = *msg;
//  object_list.clear();

  if(new_obj.inhand == false)
  {
    bool flag = true;

    for(unsigned int i = 0; i < object_list.size(); i++)
    {
      if(object_list[i].inhand == true)
      {
        object_list.erase(object_list.begin() + i);
        break;
      }
    }

//    processScene();
    while(flag && ros::ok()) {
    for(unsigned int i = 0; i < object_list.size(); i++)
    {
      ROS_INFO("i = %d",object_list[i].id);
      if(new_obj.color.r == object_list[i].color.r && new_obj.color.b == object_list[i].color.b)
      {
        flag = false;
        object_list[i].header.stamp = ros::Time::now();
        object_list[i].id = new_obj.id;
        break;
      }
    }
    ros::spinOnce();
    ros::Duration(0.05f).sleep();
    ROS_INFO("Here");
    }
  }
  else {
//    processScene();
    for(unsigned int i = 0; i < object_list.size(); i++)
    {
      if(object_list[i].id== new_obj.id)
      {
        object_list[i].header.frame_id = new_obj.header.frame_id;
        object_list[i].header.stamp = ros::Time::now();
        object_list[i].pose = new_obj.pose;
        object_list[i].inhand = new_obj.inhand;
      }
    }
  }
}
*/

bool ObjectManager::tableRequestCallback(object_manager::RequestTable::Request& request,object_manager::RequestTable::Response& response)
{
  response.table = table;
  return true;
}

void ObjectManager::spin()
{
  object_manager::ObjectList objlist, objlist_global, objlist_active;
  initializeToolbox();
  processScene();

  while(ros::ok())
  {
    if (update_status){
      // timing - ~ 2 hz, why?
      static ros::Time last_update = ros::Time::now();
      ros::Duration delta = ros::Time::now() - last_update;
      //std::cout << "SPIN loop: delta_t: " << delta.toSec() << " hz: " << 1.0/delta.toSec() << "\n";
      last_update = ros::Time::now();

      // tbd - see how fast we can process
      processScene();
 
      // combine object_list and object_list_toolbox into object_list_all
      object_list_all = object_list;
      object_list_all.insert(object_list_all.end(), object_list_toolbox.begin(), object_list_toolbox.end());      

      // publish objects
      objlist.object_list.resize(object_list_all.size());
      for(unsigned int i = 0; i < object_list_all.size(); i++)
        objlist.object_list[i] = object_list_all[i];
      objlist.table = table;
      object_all_publisher.publish(objlist);

      // publish active objects
      objlist_active.object_list.resize(object_list.size());
      for(unsigned int i = 0; i < object_list.size(); i++)
        objlist_active.object_list[i] = object_list[i];
      objlist_active.table = table;
      object_publisher.publish(objlist_active);

      // publish global objects
      objlist_global.object_list.resize(object_list_all.size());
      for(unsigned int i = 0; i < object_list_all.size(); i++)
        objlist_global.object_list[i] = object_list_all[i];
      // transform
      try {
        ros::Time now = ros::Time::now();
        listener.waitForTransform("/odom_combined", "/base_link", ros::Time::now(), ros::Duration(3.0));
        for(unsigned int i = 0; i < objlist.object_list.size(); i++) {
          geometry_msgs::PoseStamped ps_in, ps_out;
          //object_list[i].header.stamp = ros::Time::now();
          ps_in.header = objlist.object_list[i].header;
          ps_in.header.stamp = now;
          ps_in.pose = objlist.object_list[i].pose;
          // malformed quat error --- try resetting
          ps_in.pose.orientation.x = 0;
          ps_in.pose.orientation.y = 0;
          ps_in.pose.orientation.z = 0;
          ps_in.pose.orientation.w = 1.0;
          //listener.waitForTransform("/odom_combined", "/base_link", ros::Time::now(), ros::Duration(3.0));
          listener.transformPose("/odom_combined", ps_in, ps_out);
          //printf("POSE OUT: x: %f y: %f z: %f\n", ps_out.pose.position.x, ps_out.pose.position.y, ps_out.pose.position.z);
          objlist_global.object_list[i].header.frame_id = "/odom_combined";
          objlist_global.object_list[i].pose = ps_out.pose;
        }
        //fprintf(stderr, "trying table\n");
        geometry_msgs::PoseStamped tab_out;
        listener.transformPose("/odom_combined", table.pose, tab_out);
        objlist_global.table = table;
        objlist_global.table.pose = tab_out;
        object_global_publisher.publish(objlist_global);
      } catch (tf::TransformException ex) {
        printf("ERROR: TRANSFORM NOT FOUND: %s\n", ex.what());
      }
      // publish markers
      publishObjectMarkers();
      drawTable(table);


      /*
      // maintain transform from gripper to base_link
      //tf::StampedTransform transform;
      try {
        //listener.lookupTransform("/r_gripper_tool_frame", "/base_link", ros::Time(0), transform);
        //printf("TRANSFORM R->B: x: %f, y: %f, z: %f\n", transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
        listener.lookupTransform("/base_link", "/r_gripper_tool_frame", ros::Time(0), gripper2base_transform);
        printf("TRANSFORM B->R: x: %f, y: %f, z: %f\n", gripper2base_transform.getOrigin().x(), gripper2base_transform.getOrigin().y(), gripper2base_transform.getOrigin().z());

        listener.lookupTransform("/odom_combined", "/base_link", ros::Time(0), base2odom_transform);
        printf("TRANSFORM O->B: x: %f, y: %f, z: %f\n", base2odom_transform.getOrigin().x(), base2odom_transform.getOrigin().y(), base2odom_transform.getOrigin().z());

        //tf_base2odom_listener
      } catch (tf::TransformException ex) {
        printf("ERROR: TRANSFORM NOT FOUND: %s\n", ex.what());
      }
      */
    }
    ros::spinOnce();
    //ros::Duration(0.1f).sleep();
  }
}

/* Debugging function */
void ObjectManager::publishSegmentedPoints(std::vector<sensor_msgs::PointCloud2> clusters)
{
  sensor_msgs::PointCloud2 pc2;
  sensor_msgs::PointCloud2 final;

  final.height = 1;
  for(unsigned int i =0; i < clusters.size(); i++)
  {
    pc2 = clusters[i];
    final.header = pc2.header;
    final.fields = pc2.fields;
    final.width += pc2.width;
    final.is_bigendian = pc2.is_bigendian;
    final.point_step = pc2.point_step;
    final.row_step += pc2.row_step;
    final.data.insert(final.data.end(),pc2.data.begin(),pc2.data.end());
  }
  point_publisher.publish(final);
}







//
// Drawing Functions
//

void ObjectManager::drawTable(tabletop_segmentor::Table& table)
{
  visualization_msgs::MarkerArray marray;
  marray.markers.resize(1);
  marray.markers[0].header = table.pose.header;
  marray.markers[0].header.stamp = ros::Time::now();

  marray.markers[0].ns = "table";
  marray.markers[0].id = 99;
  marray.markers[0].type = visualization_msgs::Marker::TRIANGLE_LIST;
  marray.markers[0].action = visualization_msgs::Marker::ADD;
  marray.markers[0].pose = table.pose.pose;
  marray.markers[0].points.resize(6);
  marray.markers[0].scale.x = 1;
  marray.markers[0].scale.y = 1;
  marray.markers[0].scale.z = 1;
  marray.markers[0].color.r = 0.5;
  marray.markers[0].color.g = 0.5;
  marray.markers[0].color.b = 0.5;
  marray.markers[0].color.a = 0.5;

  marray.markers[0].points[0].x = table.x_min;
  marray.markers[0].points[0].y = table.y_min;
  marray.markers[0].points[0].z = 0.0f;
  marray.markers[0].points[1].x = table.x_min;
  marray.markers[0].points[1].y = table.y_max;
  marray.markers[0].points[1].z = 0.0f;
  marray.markers[0].points[2].x = table.x_max;
  marray.markers[0].points[2].y = table.y_max;
  marray.markers[0].points[2].z = 0.0f;

  marray.markers[0].points[3].x = table.x_max;
  marray.markers[0].points[3].y = table.y_max;
  marray.markers[0].points[3].z = 0.0f;
  marray.markers[0].points[4].x = table.x_max;
  marray.markers[0].points[4].y = table.y_min;
  marray.markers[0].points[4].z = 0.0f;
  marray.markers[0].points[5].x = table.x_min;
  marray.markers[0].points[5].y = table.y_min;
  marray.markers[0].points[5].z = 0.0f;

  marker_publisher.publish(marray);
}

void ObjectManager::publishObjectMarkers()
{

  // print out rate --- ~10hz
  static ros::Time last_update = ros::Time::now();
  ros::Duration delta = ros::Time::now() - last_update;
  //std::cout << "DELTA: " << delta.toSec() << "\n";
  last_update = ros::Time::now();


  //std::cout << "Publishing Object Markers\n";
  visualization_msgs::MarkerArray marray;
  marray.markers.resize(2*object_list_all.size()); // cubes + labels = 2x

  unsigned int i;
  // setup markers for Bounding Boxes / CUBES
  for(i=0; i < object_list_all.size(); i++)
  {
    //std::cout << i << "\n";
    marray.markers[i].header = object_list_all[i].header;
    marray.markers[i].header.stamp = ros::Time::now();
    marray.markers[i].ns = "bounding_boxes";
    marray.markers[i].id = object_list_all[i].id;
    marray.markers[i].type = visualization_msgs::Marker::CUBE;
    marray.markers[i].pose = object_list_all[i].pose;
    marray.markers[i].scale = object_list_all[i].dim;

    // set the marker color to grey if the object is currently being transfered from the toolbox to the active area
    if (object_list_all[i].color.r == -1){
	marray.markers[i].color.r = 125;
	marray.markers[i].color.g = 125;
	marray.markers[i].color.b = 125;
    } else
    	marray.markers[i].color = object_list_all[i].color;

    if (!object_list_all[i].atHome)
    {
      marray.markers[i].action = object_draw_type[i]; //visualization_msgs::Marker::ADD;
      // if object is in the active area and  not visible, set low alpha
      if (!object_visibility_mask[i].visible) {
        // fade alpha as fxn of time (0 seconds = 0.9, 10 seconds = 0.1)
        ros::Duration d = ros::Time::now() - object_visibility_mask[i].last_time_seen;
        double scale = 1.0 - std::min(d.toSec(), 10.0) / 10.0; // [0.0 1.0], 10 sec's max
        marray.markers[i].color.a = 0.3 + scale*0.6;
      } 
    } else marray.markers[i].action = visualization_msgs::Marker::ADD;
  }

  // setup markers for LABELS
  for(i=object_list_all.size(); i < 2*object_list_all.size(); i++) {
    double block_ind = i - object_list_all.size();
    marray.markers[i].header = object_list_all[block_ind].header;
    marray.markers[i].header.stamp = ros::Time::now();
    marray.markers[i].ns = "bounding_box_labels";
    marray.markers[i].id = object_list_all[block_ind].id + 100; // tbd - need unique label?
    // int2str
    std::string buf;
    std::stringstream bout;

    if(!object_list_all[block_ind].atHome) {
      // if in active area and not visible, add parentheses around label
      if (!object_visibility_mask[block_ind].visible)
	bout << "(" << object_list_all[block_ind].id << ")";
      else
	bout << object_list_all[block_ind].id;
    }
    else
      bout << "[" << object_list_all[block_ind].id << "]";  // if in toolbox, add brackets around label

    // get color
    bout << "_" << object_list_all[block_ind].label;
    //std::cout << bout.str() << "\n";
    buf = bout.str();
    marray.markers[i].text = buf.c_str();
    marray.markers[i].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    // handle draw type
    if (!object_list_all[block_ind].atHome) marray.markers[i].action = object_draw_type[block_ind]; //visualization_msgs::Marker::ADD;
    else marray.markers[i].action = visualization_msgs::Marker::ADD;
    marray.markers[i].pose = object_list_all[block_ind].pose;
    marray.markers[i].pose.position.z += 0.05; // label 5 cm above cube
    marray.markers[i].scale = object_list_all[block_ind].dim;
    marray.markers[i].color.r = 1; // = object_list[i].color;
    marray.markers[i].color.g = 1; // = object_list[i].color;
    marray.markers[i].color.b = 1; // = object_list[i].color;
    if (( !object_list_all[block_ind].atHome) && (!object_visibility_mask[i].visible ))
      marray.markers[i].color.a = 0.5f; // = object_list[i].color;
    else
      marray.markers[i].color.a = 1.0;
  }

  marker_publisher.publish(marray);
  //  ROS_INFO("Object Published");
}

void ObjectManager::removeOldMarkers()
{
  visualization_msgs::MarkerArray marray;
  for(unsigned int i =0; i < object_list.size(); i++)
  {
    visualization_msgs::Marker m;
    m.header.stamp = ros::Time::now();
    m.header.frame_id = object_list[i].header.frame_id;
    m.id = object_list[i].id;
    m.action = visualization_msgs::Marker::DELETE;
    m.ns = "bounding_boxes";
    marray.markers.push_back(m);
  }

  // TBD - also handle labels

  marker_publisher.publish(marray);

}
